#include <remote_task.h>

#include "bsp_dwt.h"
#include "cmsis_os.h"
#include "demo.h"
#include "ps2.h"
#include "tools.h"

RobotControl *robot_ctrl_pointer = NULL;  // 机器人控制指针
const ps2data_t *ps2_pointer = NULL;      // ps2手柄数据指针

//--------------------本地函数声明------------------
/// @brief 右键盘区域按键检测
/// @param  none
static void right_keyboard_dectect(void);

/// @brief 左键盘区域按键检测
/// @param  none
static void left_keyboard_dectect(void);

/// @brief 肩部区域按键检测
/// @param  none
static void shoulder_keyboard_dectect(void);

//--------------------线程主函数实现------------------
void remote_task(void const *argument) {
    // 获取机器人控制指针
    robot_ctrl_pointer = get_rebot_control_pointer();

    // 获取PS2数据指针
    ps2_pointer = get_ps2data_pointer();

    while (1) {
        pstwo_read();  // PS2数据读取

        // 有按键按下时
        if (ps2_pointer->key != 0) {
            // 右键盘区域
            right_keyboard_dectect();

            // 左键盘区域
            left_keyboard_dectect();

            // 肩部区域
            shoulder_keyboard_dectect();
        }

        // 震动反馈
        if (robot_ctrl_pointer->start_loop) {
            PS2_Vibration(0x01, 0xFB);
        } else {
            PS2_Vibration(0x00, 0x00);
        }
        osDelay(10);
    }
}

//--------------------本地函数实现------------------
static void right_keyboard_dectect(void) {
    switch (ps2_pointer->key) {
        case PSB_TRIANGLE:
            robot_ctrl_pointer->foot_style = TRIPEDAL;
            break;

        case PSB_CIRCLE:
            robot_ctrl_pointer->foot_style = FIVE_LEG;
            break;

        case PSB_CROSS:
            // toggle the start_loop state
            robot_ctrl_pointer->start_loop =
                !robot_ctrl_pointer->start_loop;  // 开始或停止运动

            // 等待按键松开
            wait_until_block_DWT(!PS2_get_button_state(PSB_CROSS));
            break;

        default:
            break;
    }
}

static void left_keyboard_dectect(void) {
    switch (ps2_pointer->key) {
        case PSB_PAD_UP:
            robot_ctrl_pointer->yaw_change = -15;  // 增加抬起角度
            break;

        case PSB_PAD_DOWN:
            robot_ctrl_pointer->yaw_change = 15;  // 减少抬起角度
            break;

        case PSB_PAD_LEFT:
            robot_ctrl_pointer->yaw_fix -= 5;  // 减少迈开角度
            // 等待按键松开
            wait_until_block_DWT(!PS2_get_button_state(PSB_PAD_LEFT));
            break;

        case PSB_PAD_RIGHT:
            robot_ctrl_pointer->yaw_fix += 5;  // 增加迈开角度
            // 等待按键松开
            wait_until_block_DWT(!PS2_get_button_state(PSB_PAD_RIGHT));
            break;

        default:
            break;
    }
}

static void shoulder_keyboard_dectect(void) {
    switch (ps2_pointer->key) {
        case PSB_L1:
            robot_ctrl_pointer->pump_all_on = 0;  // 关闭气泵
            break;

        case PSB_L2:
            robot_ctrl_pointer->pump_all_on = 1;  // 开启气泵
            break;

        case PSB_R1:
            robot_ctrl_pointer->glue = 0;  // 关闭胶枪
            break;

        case PSB_R2:
            robot_ctrl_pointer->glue = 1;  // 开启胶枪
            break;

        default:
            break;
    }
}
